#include "sticks.h"
#include "uart.h"
#include "rc.h"
#include "speaker.h"
#include "os_tick.h"
#include "rgb.h"
#include "config.h"
#define LOG_D LOG
#define ABS(x) (((x) > 0) ? (x) : -(x))
enum {
    RF_TEST_FLAG = 0x01,
    BP_TEST_FLAG = 0x02,
    CALI_MIN_FLAG = 0x04,
    CALI_MAX_FLAG = 0x08,
};
#define CALI_MIN_FLAG 0X01
#define CALI_MAX_FLAG 0X02
#define CALI_MID_FLAG 0X04
uint8_t min_holdTime[4] = { 0 };
uint8_t max_holdTime[4] = { 0 };
uint8_t test_count[8] = { 0 };
int16_t applyDeadband(int16_t value, int16_t deadband)
{
    if (ABS(value) < deadband)
    {
        value = 0;
    }
    else if (value > 0)
    {
        value -= deadband;
    }
    else if (value < 0)
    {
        value += deadband;
    }
    return value;
}
int16_t constrain(int16_t value, int16_t min, int16_t max)
{
    if (value > max)
        return max;
    if (value < min)
        return min;
    return value;
}
int16_t limit(int16_t value, int16_t rang)
{
    if (value > rang) return rang;
    if (value < -rang) return -rang;
    return value;
}
void stick_cali_on()
{
    config_list.testFlag1 = 0;
    rcSticks.state =
        RC_STICK_MID_CAL_ON_FLAG
        | RC_STICK_MIN_CAL_ON_FLAG
        | RC_STICK_MAX_CAL_ON_FLAG
        | RC_STICK_BUTTON_CAL_ON_FLAG;

    rcSticks.llr.caliMin = 0;rcSticks.llr.caliMax = 0;
    rcSticks.lud.caliMin = 0;rcSticks.lud.caliMax = 0;
    rcSticks.rlr.caliMin = 0;rcSticks.rlr.caliMax = 0;
    rcSticks.rud.caliMin = 0;rcSticks.rud.caliMax = 0;

    RGB_STATE_SET(RGB_STATE_RC_CALI);
    LOG("stick_cali_on\n");
}
void stick_cali_off()
{
    rcSticks.state = 0;
    config_list.testFlag1 = TEST_FLAG_CALIBRATED;
    config_save_all();

    RGB_STATE_CLR(RGB_STATE_RC_CALI);
    RGB_STATE_CLR(RGB_STATE_OP_TEST_ERROR);

    LOG("stick_cali_off\n");
    speaker_out(523, 250);//DO_M 
    delay_ms(250);
    speaker_out(587, 250);//RE_M
    delay_ms(250);
    speaker_out(659, 250);//MI_M

}

#define RC_VALUE_DT 50
/**
 * @brief update latest Adc value to sticks
 *
 * @param rlr
 * @param rud
 * @param llr
 * @param lud
 */
void sticks_update(rc_sticks_t* s, uint16_t rlr, uint16_t rud, uint16_t llr, uint16_t lud)
{
    struct stick_channel* channel[4];
    int16_t adc[4];
    uint8_t i = 0;
    // static uint8_t debug_cnt = 0;
    static uint8_t buttonTest = 0;
    static uint8_t MidCaliFlag = 0;
    static uint8_t RangCaliFlag = 0;

    channel[0] = &s->llr;
    channel[1] = &s->lud;
    channel[2] = &s->rlr;
    channel[3] = &s->rud;
    adc[0] = llr;
    adc[1] = lud;
    adc[2] = rlr;
    adc[3] = rud;

    for (i = 0;i < 4;i++)
    {
        //NOTE Low pass filter
        channel[i]->adc = adc[i];
        channel[i]->lpf = channel[i]->lpf * 0.5f + channel[i]->adc * 0.5f;
        // channel[i]->lpf = adc[i];
        if (config_list.testFlag1 == TEST_FLAG_CALIBRATED)
        {
            //NOTE apply new min, max
            channel[i]->value = channel[i]->lpf - config_list.rcMid[i];
            channel[i]->value = STICK_APPLY_RANGE(channel[i]->value, config_list.rcMin[i], config_list.rcMax[i]);
            channel[i]->value = applyDeadband(channel[i]->value, channel[i]->deadBand);
            channel[i]->value = limit(channel[i]->value, 512);
            //NOTE range:0~1024, mid:512
            channel[i]->value += 512;
            // LOG("\nTEST_FLAG_CALIBRATED\n");
        }
        else
        {
            //NOTE sum up mid value
            if ((s->state & RC_STICK_MID_CAL_ON_FLAG))
            {

                // LOG("1\n");
                // LOG("\n RC_STICK_MID_CAL_ON_FLAG %d\n",channel[i]->lpf);
                if ((channel[i]->lpf) < 768 && (channel[i]->lpf > 256) && millis() > 1000)
                {
                    if (channel[i]->sum_cnt == 0)
                    {
                        channel[i]->value = 0;
                        channel[i]->mid = 0;
                    }
                    channel[i]->mid += channel[i]->lpf;
                    channel[i]->sum_cnt++;
                    // LOG("channel[%d]->mid:%ld,%d\n", (int)i, channel[i]->mid, (int)channel[i]->sum_cnt);
                    if (channel[i]->sum_cnt >= 50)
                    {
                        //NOTE get mid value
                        channel[i]->mid = channel[i]->mid / channel[i]->sum_cnt;
                        channel[i]->sum_cnt = 0;
                        MidCaliFlag |= (0x01 << i);

                    }

                }
                else
                {
                    channel[i]->sum_cnt = 0;
                }
                // LOG_D("MidCaliFlag:0x%0X\n", (int)MidCaliFlag);
                if (MidCaliFlag == 0x0F)
                {
                    s->state &= ~RC_STICK_MID_CAL_ON_FLAG;
                    LOG_D("\n>> mid cali done: %ld %ld %ld %ld\n", channel[0]->mid, channel[1]->mid, channel[2]->mid, channel[3]->mid);
                    speaker_out(526, 200);
                    delay_ms(200);
                    MidCaliFlag = 0;
                }
            }
            else
            {
                // LOG("\RC_STICK_MIN_CAL_ON_FLAG\n");
                //NOTE origin to 0
                // LOG("2\n");
                MidCaliFlag = 0;
                channel[i]->value = channel[i]->lpf - channel[i]->mid;
                // LOG_D("stick:%02X %02X %d \t %d \t %d \t %d key:%X\n", (int)RangCaliFlag, (int)s->state, channel[0]->value, channel[1]->value, channel[2]->value, channel[3]->value, keyValue);
                //NOTE Cali min
                if (s->state & RC_STICK_MIN_CAL_ON_FLAG)
                {
                    //[-1024,-256],change < 10,hold 1s
                    // LOG("Min\n");
                    if ((channel[i]->value < -256) && (channel[i]->value > -1024))
                    {
                        if (ABS(channel[i]->value - channel[i]->preValue) < RC_VALUE_DT)
                        {
                            if (min_holdTime[i] < 10)
                            {
                                min_holdTime[i]++;
                                channel[i]->caliMin = 0;
                                LOG("\nmin_holdTime[%d]:%d %d\n", (int)i, (int)min_holdTime[i], channel[i]->value);
                            }
                            else
                            {
                                channel[i]->caliMin = channel[i]->value;
                                RangCaliFlag |= (0x01 << i);
                                LOG("\m min_holdTime[%d]:%d %d\n", (int)i, (int)min_holdTime[i], channel[i]->caliMin);
                                speaker_out(2000 + i * 56, 200);
                                delay_ms(200);
                            }
                        }
                        else
                        {
                            min_holdTime[i] = 0;
                        }
                    }
                    else
                    {
                        min_holdTime[i] = 0;
                    }
                }
                //NOTE Cali max
                if (s->state & RC_STICK_MAX_CAL_ON_FLAG)
                {
                    // LOG("Max\n");
                    //[256,1024],change < 10,hold 1s
                    if ((channel[i]->value > 256) && (channel[i]->value < 1024))
                    {
                        if (ABS(channel[i]->value - channel[i]->preValue) < RC_VALUE_DT)
                        {
                            if (max_holdTime[i] < 10)
                            {
                                max_holdTime[i]++;
                                channel[i]->caliMax = 0;
                                // LOG("\nmax_holdTime[%d]:%d %d\n", (int)i, (int)max_holdTime[i], channel[i]->value);
                            }
                            else
                            {
                                channel[i]->caliMax = channel[i]->value;
                                RangCaliFlag |= (0x01 << (i + 4));
                                LOG("\n max_holdTime[%d]:%d %d\n", (int)i, (int)max_holdTime[i], channel[i]->caliMax);
                                speaker_out(2500 + i * 56, 200);
                            }
                        }
                        else
                        {
                            max_holdTime[i] = 0;
                        }
                    }
                    else
                    {
                        max_holdTime[i] = 0;
                    }
                }
            }//max
            // channel[i]->value = constrain(channel[i]->value+512, 0, 1023);
        }//calibrate

        //NOTE IDLE TRACK
        if (channel[i]->preValue != channel[i]->value)
        {
            if (ABS(channel[i]->value - channel[i]->preValue) > 50)
            {
                //NOTE stop system shutdown
                ResetSystemShutdownCountdown();
            }
            channel[i]->preValue = channel[i]->value;
            // LOG("channel[%d]:%d\n", (int)i, channel[i]->value);
            // LOG_D("stick: %d \t %d \t %d \t %d key:%d\n", channel[0]->value, channel[1]->value, channel[2]->value, channel[3]->value, keyValue);
        }
    }//for

    if (s->state)
    {

        if (RangCaliFlag == 0xff)
        {
            s->state &= ~RC_STICK_MIN_CAL_ON_FLAG;
            s->state &= ~RC_STICK_MAX_CAL_ON_FLAG;
            // LOG("get min:%d %d %d %d\n", channel[0]->caliMin, channel[1]->caliMin, channel[2]->caliMin, channel[3]->caliMin);
            // LOG("get max:%d %d %d %d\n", channel[0]->caliMax, channel[1]->caliMax, channel[2]->caliMax, channel[3]->caliMax);

        }

        if (keyAction == 0x11) { buttonTest |= 0x01;keyAction = 0;speaker_out(560, 100); }
        if (keyAction == 0x21) { buttonTest |= 0x02;keyAction = 0;speaker_out(660, 100); }
        if (keyAction == 0x31) { buttonTest |= 0x04;keyAction = 0;speaker_out(760, 100); }
        if (keyAction == 0x41) { buttonTest |= 0x08;keyAction = 0;speaker_out(860, 100); }
        if (keyAction == 0x51) { buttonTest |= 0x10;keyAction = 0;speaker_out(960, 100); }
        if (buttonTest == 0x1F)
        {
            rcSticks.state &= ~RC_STICK_BUTTON_CAL_ON_FLAG;
            // LOG("button test ok!\n");
        }
        if (!s->state)
        {
            config_list.testFlag1 = TEST_FLAG_CALIBRATED;
            config_list.rcMax[0] = channel[0]->caliMax;
            config_list.rcMax[1] = channel[1]->caliMax;
            config_list.rcMax[2] = channel[2]->caliMax;
            config_list.rcMax[3] = channel[3]->caliMax;

            config_list.rcMin[0] = channel[0]->caliMin;
            config_list.rcMin[1] = channel[1]->caliMin;
            config_list.rcMin[2] = channel[2]->caliMin;
            config_list.rcMin[3] = channel[3]->caliMin;

            config_list.rcMid[0] = channel[0]->mid;
            config_list.rcMid[1] = channel[1]->mid;
            config_list.rcMid[2] = channel[2]->mid;
            config_list.rcMid[3] = channel[3]->mid;

            LOG("Saving cali value:\n");
            LOG("Min:%d %d %d %d\n", config_list.rcMin[0], config_list.rcMin[1], config_list.rcMin[2], config_list.rcMin[3]);
            LOG("Mid:%d %d %d %d\n", config_list.rcMid[0], config_list.rcMid[1], config_list.rcMid[2], config_list.rcMid[3]);
            LOG("Max:%d %d %d %d\n", config_list.rcMax[0], config_list.rcMax[1], config_list.rcMax[2], config_list.rcMax[3]);
            // config_save_all();
            stick_cali_off();
        }
    }
    else
    {
        buttonTest = 0;
    }

    // if (debug_cnt++ < 2) return;
    // debug_cnt = 0;

    // uart1_puc(0x03);
    // uart1_puc(0xfc);
    // uart1_puc(channel[0]->value);uart1_puc(channel[0]->value >> 8);
    // uart1_puc(channel[0]->lpf);uart1_puc(channel[0]->lpf>>8);            
    // uart1_puc(channel[0]->mid);uart1_puc(channel[0]->mid>>8);            
    // uart1_puc(channel[1]->value);uart1_puc(channel[1]->value >> 8);
    // uart1_puc(channel[1]->lpf);uart1_puc(channel[1]->lpf >> 8);
    // uart1_puc(channel[1]->mid);uart1_puc(channel[1]->mid >> 8);
    // uart1_puc(channel[2]->value);uart1_puc(channel[2]->value >> 8);
    // uart1_puc(channel[2]->mid);uart1_puc(channel[2]->mid>>8);            
    // uart1_puc(channel[2]->lpf);uart1_puc(channel[2]->lpf>>8);            
    // uart1_puc(channel[3]->value);uart1_puc(channel[3]->value >> 8);
    // uart1_puc(channel[3]->lpf);uart1_puc(channel[3]->lpf>>8);                               
    // uart1_puc(channel[3]->mid);uart1_puc(channel[3]->mid>>8);                               
    // uart1_puc(0xfc);
    // uart1_puc(0x03);
    // LOG_D("adc: %d \t %d \t %d \t %d \t", channel[0]->adc, channel[1]->adc, channel[2]->adc, channel[3]->adc);
    // LOG_D("lpf: %04d %04d %04d %04d ", channel[0]->lpf, channel[1]->lpf, channel[2]->lpf, channel[3]->lpf);
    // LOG_D("max: %04d %04d %04d %04d ", channel[0]->max, channel[1]->max, channel[2]->max, channel[3]->max);
    // LOG_D("min: %04d %04d %04d %04d ", channel[0]->min, channel[1]->min, channel[2]->min, channel[3]->min);
    // LOG_D("val: %04d %04d %04d %04d\n", channel[0]->value, channel[1]->value, channel[2]->value, channel[3]->value);
    // LOG_D("adc:%d %d %d %d \t", channel[0]->adc, channel[1]->adc, channel[2]->adc, channel[3]->adc);

}


